/**
* \brief 
* \author pengcheng (pengcheng@yslrpch@126.com)
* \date 2020-05-20
* \attention CopyrightÃ‚Â©ADC Technology(tianjin)Co.Ltd
* \attention Refer to COPYRIGHT.txt for complete terms of copyright notice
*/

#ifndef  STEREO_VISION_SGM_APP_H
#define STEREO_VISION_SGM_APP_H


#include <opencv2/core/core.hpp>
#include <queue>
#include <utility>
#include <mutex>
#include <functional>
#include <cuda_runtime.h>
#include <memory>
#include <stereo_vision/sgm.h>
#include <cmw_app/work.h>
namespace adc
{


struct device_buffer
{
	device_buffer() : data(nullptr) {}
	device_buffer(size_t count) { allocate(count); }
	void allocate(size_t count) { cudaMalloc(&data, count); }
	~device_buffer() { cudaFree(data); }
	void* data;
};


struct CameraParameters
{
	float fu;
	float fv;
	float u0;
	float v0;
	float baseline;
	float height;
	float tilt;
    float pitch;
    float yaw;

};

struct CoordinateTransform
{
	CoordinateTransform(const CameraParameters& camera) : camera(camera)
	{
		sinTilt = (sinf(camera.tilt));
		cosTilt = (cosf(camera.tilt));
		bf = camera.baseline * camera.fu;
		invfu = 1.f / camera.fu;
		invfv = 1.f / camera.fv;
        sin_pitch = (sinf(camera.pitch));
        cos_pitch = (cosf(camera.pitch));
        sin_yaw = (sinf(camera.yaw));
        cos_yaw = (cosf(camera.yaw));
        cv::Mat R_x = (cv::Mat_<float>(3,3)<<1 ,    0 ,                 0,
                                                                                    0,    sin_pitch,    cos_pitch,
                                                                                    0,    -sin_pitch,  cos_pitch);
        cv::Mat R_y = (cv::Mat_<float>(3,3)<<cos_yaw,    0,     -sin_yaw,
                                                                                    0 ,                  1 ,                    0,
                                                                                    sin_yaw,     0,     cos_yaw);
        cv::Mat R_z = (cv::Mat_<float>(3,3)<<cosTilt,     sinTilt,        0,
                                                                                -sinTilt,     cosTilt,      0,
                                                                                0,                  0,                  1);
        R = R_x*R_y*R_z;                                                                        
	}

	inline cv::Point3f imageToWorld(const cv::Point2f& pt, float d) const
	{
		const float u = pt.x;
		const float v = pt.y;

		const float Zc = bf / d;
		const float Xc = invfu * (u - camera.u0) * Zc;
		const float Yc = invfv * (v - camera.v0) * Zc;
        // cv::Mat point = (cv::Mat_<float>(3,1)<<Xc,Yc,Zc);
        // point = R*point;
        // float x_w = point.at<float>(0,0);
        // float y_w =point.at<float>(1,0);
        // float z_w = point.at<float>(2,0);
		return cv::Point3f(Xc, Yc, Zc);
	}

	CameraParameters camera;
	float sinTilt, cosTilt, bf, invfu, invfv;
    float  sin_pitch,  cos_pitch, sin_yaw,cos_yaw;
    cv::Mat R;
};


class StereoSgmApp : public Worker
{

public:
    typedef std::vector<cv::Vec3b> PointColorType;
    typedef std::vector<cv::Point3f> PointCloudType;
    typedef std::chrono::system_clock::time_point  TimeType;
    typedef cv::Mat PointCloudMat;
    typedef std::function<void(PointCloudMat&, TimeType&)> PointCloudMatCallback;
    typedef std::function<void(PointCloudType&, TimeType&)> PointCloudCallback;
    typedef std::function<void(PointColorType&, TimeType&)> PointColorCallback;
    typedef std::function<void(PointCloudType&,  PointColorType&, TimeType&)> PointAndColorCallback;

    explicit StereoSgmApp(const std::string &config_file, const float hz) ;

    void SetPointCloudCallback(PointCloudCallback callback);
    void SetPointColorCallback(PointColorCallback callback);
    void SetPointAdnColorCallback(PointAndColorCallback callback);
    void SetPointCloudMatCallback(PointCloudMatCallback callback);
    void DoWork();
    void CacheImages(const cv::Mat &left_img, const cv::Mat &right_img, TimeType& time);
    bool ReceiveImage(void);
    static void ReadParams(const std::string& yaml_file, cv::Mat& K, cv::Mat& D, cv::Mat& P, cv::Mat& R, cv::Size& size);
private:
    void InitConfig(const std::string& config_file);

    void ReprojectPointsTo3D();

    void StereoMatch();
private:
    std::mutex mutex_image_;
private:
    ///cache data
    std::queue<std::pair<cv::Mat,cv::Mat>> image_cache_;
    std::queue<std::chrono::system_clock::time_point> time_cache_;
private:
    cv::Mat map_l_x, map_l_y, map_r_x, map_r_y;
    std::shared_ptr<CameraParameters> camera_ptr_;
    std::shared_ptr<CoordinateTransform> tf_ptr_;
    std::shared_ptr<adc::StereoSGM> sgm_ptr_;
    float max_depth_;
    int input_type_;
    int input_bytes_;
    int output_bytes_;

    int disp_size_ = 64;
    int output_depth_ = 16;
    bool subpixel_ = true;
    int P1_ = 10;
    int P2_ = 120;
    float uniqueness_ = 0.95f;
    int top_row_ = 0;

    const size_t kQueueSize = 10;

    cv::Mat dispartity_, dispartity_32f_;
    cv::Mat left_image_, right_image_;
    std::chrono::system_clock::time_point  current_time_;

    std::shared_ptr<device_buffer> d_left_ptr_;
    std::shared_ptr<device_buffer> d_right_ptr_;
    std::shared_ptr<device_buffer> d_dispartity_ptr_;
    PointCloudType point_cloud_;
    PointColorType point_color_;
    cv::Mat point_cloud_mat_;


    PointColorCallback point_color_callback_;
    PointCloudCallback point_cloud_callback_;
    PointAndColorCallback poind_and_color_callback_;
    PointCloudMatCallback point_cloud_mat_callback_;
};

}

#endif // ! STEREO_VISION_SGM_APP_H


